#ifndef EARS_NODE
#define EARS_NODE

#include <ros/ros.h>
#include <threemxl/C3mxlROS.h>
#include <XMLConfiguration.h>
#include <threemxl/dxlassert.h>
#include <cmath>
#include "topic/topic.h"
#include "comm/ears_msg.h"
#include "config.h"

#define EARS_SPEED	1.5	// half circle in a second

class EarsNode
{
private:
	ros::NodeHandle nh_;
	ros::Subscriber motor_sub_;
	ros::Publisher status_pub_;

	LxSerial serial_port_;	// serial port interface
	C3mxlROS *motor_;

	int ears_cstate;	// current state
	int ears_fstate;	// final state

	double motor_pos;	// position of the motor in rad
public:
	EarsNode();
	~EarsNode();

	/**
	 * Moves the motors to the start position.
	 */
	void extInit();

	/**
	 * Handle incoming messages for the ears.
	 * @param	msg	expected state for the ears.
	 */
	void earsCallback(const comm::ears::ConstPtr &msg);

	/**
	 * Initialize motor parameters.
	 */
	void init();

	/**
	 * Spin node.
	 */
	void spin();
};


#endif
